
/*
*  LivingSpot.h

NOTE: - the deltaElnamics of the motion is all done in polar coordinates (which is not right, but faster and more robust (because thenumSample variation on the measured depth_Range could make things jerky). 
- similarly, the deltaElnamics will be handled in AD coordinates directly (instead of angles, etc). We will use conversion just to get cartesian coordinates. 
*/

#ifndef LivingSpot_h
#define LivingSpot_h

#include "WProgram.h"
#include "hardwareIO.h"


#define MAX_SACCADE_SAMPLES 40 // this is the max number of points allowed by saccade (to avoid dynamic allocation of memory)

// ZONE IN CONSTRUCTION: ===================================================================================================================

// Proper_Calibration_Flag (exit of RangeCalibration function):
#define BAD_CALIBRATION 0
#define FAIR_CALIBRATION 1
#define GOOD_CALIBRATION 2

// Proper_Intensity_Flag values (calculated in the analyzeScanData function):
#define TOO_WEAK 0
#define APPROPRIATE_STRENGH  1
#define SATURATED_SIGNAL 2

// Proper_Distance_Flag (exit of ComputeRangeUnconstrained range function):
#define TOO_FAR 0
#define PROPER_RANGE 1
#define TOO_CLOSE 2

//Proper_Contrast_Flag (calculated in the analyzeScanData function):
#define GOOD_CONTRAST 0
#define BAD_CONSTRAST 1

// Radius Adjustement Modes (RadiusCorrectionMode state variable):
#define RADIUS_ADJUST 1
#define RADIUS_FIXED 0
// RADIUS of the saccade: 
#define MAXRADIUS 1000

// Laser Power Adjustement Modes (PowerCorrectionMode state variable):
#define POWER_ADJUST 1
#define POWER_FIXED 0

// Averaging modes: instant_Averaging_Mode for updating the POSITION BUFFER, and affecting or not 
// the ACTUAL TRACKING (depending on averaged_Track_Mode)
#define AVERAGING 1 // this model means that an AVERAGING FILTER is applied to the last instant_Position_Buffer_Size samples 
// (with instant_Position_Buffer_Size < INSTANT_POSITION_MAX_BUFFER_SIZE) before updating the latest position of the position buffer 
// whose size is position_Buffer_Size < POSITION_MAX_BUFFER_SIZE. The actual tracking position can be also affected, depending on the 
// averaged_Track_Mode;
#define NO_AVERAGING 0

// averaged_Track_Mode: the actual tracking position computed by the Track function can result from the instantaneous correction
// or from the averaged latests instantaneous positions (averaged using instant_Position_Buffer_Size samples)
#define NO_AVERAGED_TRACK 0
#define AVERAGED_TRACK 1

// Thresholding Mode (thresholding_Mode  state variable):
#define FIXED_THRESHOLD 0
#define AUTO_THRESHOLD 1
#define DEPTH_THRESHOLD 2

// Filtering Mode (filtering_Mode state variable) for the raw data (not binarized):
#define NO_FILTER 0
#define LOW_PASS_FILTER 1
//.. other?

// Filtering Mode for the binarized data (binary_smoothing_Mode state variable):
#define NO_MORPHO_SMOOTH 0
#define MORPHO_SMOOTH 1
//.. other?

// MODEL for the INTENSITY-TO-DISTANCE relation (state variable Distance_Intensity_Model)
#define ORDER_0 0 // this means that the model assumes PD_X and PD_Y << depth_Range_Range, and a order 0 developpement is done on the 
// real formula for the depth_Range-intensity.
#define ORDER_1 1 // order one developpement...
#define ORDER_1_CORRECTED_LENS 2 // order one developpement, but instead of a cos evolution of the voltage/intensity (Lambertian pd surface)
// an AD-HOC correction is done: cos^2, to account for the lens in front of the PD.
#define PRE_CALIBRATED_SKIN 3 // pre-calibrated model and parameters
#define ORDER_1_NO_COSINUS 4 // this is a model which does not include a cos relation for the photodetector. The model is otherwise "exact" and
// depth_Range_Range can be exactly resolved as in ORDER_1_CORRECTED_LENS, by solving a second degree equation.
#define ORDER_0_CORRECTED_LENS 5 // order 0 model with cos^exp_Cos behaviour (a global variable)

// behaviour when the target is lost (state variable lost_Target_Behaviour)
#define BACK_INITIAL_POSITION 0 //trackspot goes back to initial position
#define SPIRAL_SEARCH 1 // when the object is lost, a "spiral" search is launched
#define GROWING_SEARCH 2 // this is a simple searching mode (growing circles). 

//===================================================================================================================

// parametres a regler a la main or during autocalibration:
#define MAX_INTENSITE 140 //70
#define MIN_INTENSITE 20 //55
#define contrast_factor 1.0 // we have: MIN_CONTRAST_AD=(MAX_INTENSITE-MIN_INTENSITE)/contrast_factor
#define threshold_factor 0.57 // 0.6 is a good value

// IMPORTANT: 
#define DELAY_MIRRORS_CONTINUOUS 1//1//100 // in microseconds, this is the mechanical delay to set the mirrors. It may be LARGER WHEN THERE ARE MORE SPOTS
#define DELAY_MIRRORS_START 400 //300 // for the start of the saccade (in particular, when it comes from ANOTHER spot). 
#define EXTRA_MIRROR_SAMPLES 4 // extra samples to avoid having the first point in bad position (when several spots or other things)

//#define GRAVITYCONST 0.01
//#define MASSE 30000

//const static int tableSLPLogo_size = 53;
//const static int tableSLPLogo_X[tableSLPLogo_size] = {7.0, 7.0, 6.0, 5.0, 4.0, 3.0, 3.0, 3.0, 3.0, 4.0, 5.0, 6.0, 7.0, 7.0, 7.0, 7.0, 6.0, 5.0, 4.0, 3.0, 3.0, 3.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 1.0, 0.0, -1.0, -2.0, -2.0, -2.0, -3.0, -3.0, -3.0, -3.0, -3.0, -3.0, -3.0, -4.0, -5.0, -6.0, -7.0, -7.0, -7.0, -7.0, -6.0, -5.0, -4.0, -3.0};
//const static int tableSLPLogo_Y[tableSLPLogo_size] = {-2.0, -3.0, -3.0, -3.0, -3.0, -3.0, -2.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 3.0, 3.0, 3.0, 3.0, 2.0, 3.0, 3.0, 2.0, 1.0, 0.0, -1.0, -2.0, -3.0, -3.0, -3.0, -3.0, -3.0, -2.0, -3.0, -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0, 3.0, 3.0, 3.0, 3.0, 2.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0};

// behaviour mode (behaviourMode state variable):
#define STAND_CENTER -1
#define SIMPLE_BOUNCING 0
#define FAST_BOUNCING 1
#define NEWTON_DYNAMICS 2
#define COUNTOUR_FOLLOWING 3
#define SNELL_LAW 4
#define TRACKING 5
#define DEPTH_LETTERS 6
#define SIMPLE_BOUNCING_CENTRAL 7

// border mode (int borderMode):
#define NORMAL_REFLEXION 0
#define RETRO_REFLEXION 1
#define RESET_POSITION 2


// SOUND: 
// when touching wall: touchSoundMode
// when collision: collisionSoundMode
// when following contour: countourSoundMode
// NOTE: we can have more modes, for the time being we either use super bass or free bass: 
#define SUPER_BASS 0 
#define FREE_BASS 1

class LivingSpot
{
public:
	//public Variables

	int identifier; // (could be private to avoid modifying it)
      
        int touchSoundMode, collisionSoundMode, countourSoundMode; // to select the kind of sound it makes 

	// Behaviour mode:
	boolean alive; 
	int behaviourMode;// rem: some behaviors could be combined, but for the time being this will be avoided (this variable will change the way the update() method works)

        // Various flags:
        int Proper_Calibration_Flag; //BAD_CALIBRATION, FAIR_CALIBRATION, GOOD_CALIBRATION
        int Proper_Intensity_Flag; //TOO_WEAK, APPROPRIATE_STRENGH, SATURATED_SIGNAL 
        int Proper_Distance_Flag; // TOO_FAR, PROPER_RANGE, TOO_CLOSE 
        int Proper_Contrast_Flag; //GOOD_CONTRAST, BAD_CONSTRAST
        
	// Saccade "modelview" properties: 
        // ATTN: THERE IS A CONCEPTUAL MISTAKE HERE, we should SEPARATE the kinematic behaviour, from the MODELVIEW parameters of the saccade, that will be members
        // of a "scanning trajectory" structure or class. From the time being, The position is actually a global variable directly accessible to the behaviour routines. 
        // - POSITION: 
	//float Az, El; // Note 1: current position is private because we want to avoid out of range values - but we may need to get their value for direct sound generation, etc (not using this class methods). 
	// Note 2: Az and El are in AD units (this is best to simplify the program, as well as make motion more robust when depth_Range is computed in real time)
	// - ROTATION: 
        float saccadeRotation;
        // - SIZE: 
        float Aper_DA;

        float Az_Default, El_Default; // these are the coordinates of a special position (the initial coordinates, the center of attraction, etc). 
        float Az_old, El_old; // for newtoninan dynamics: this is the position at time t-dt (for verdet integration). The position at time t is (Az, El) and are PRIVATE variables.	
	float initVx, initVy; // initial speed (in fact, ANGULAR speed, in AD units)
	float vx, vy; //current speed (rem: use updateSpeed when newtonian dynamics because of the Verdet integration)
	float ax, ay; // current acceleration

        // Computed quantities: 
        float depth_Range; // this is the z coordinate (could be also a member of the scanning trajectory object, updatable with a call to ComputeRange
	boolean touchWall; // detection of wall borders
	boolean collision;// detection of collision with other particules
	boolean onContour; // this is to indicate that the spot is on a contour, or has lost it. 
	// collision detection (meaning that another particle is in nearby, inside collisionRadius)
	float collisionRadius; // must be calibrated!

	// Things specific to each behaviour mode (rem: in the future, we could have a subclass for each mode!)
	// bouncing: 
	float thresholdFactorFB; //threshold factor used by the fast bouncing (deprecated????)
	// contour following: 
	float speedContourFollowing; //"speed" factor used on moveAlongContour (percentage of the current radius)
	float depthThreshold; // this is for contour following on contour lines (instead of black/white objects, in this case we would use autoThreshold). 
	int contourFollowingMode; // can be CONTRAST or COUNTOUR_LINES (the first case if for high contrat, flat surfaces roughly perpendicular to the laser system; in the second, 
	// we will follow contour lines on 3d objects - this means that in the first case, we can use autoThreshold for discrimination of white and black "zones" directly from the
	// measured intensity, but in the latter we need to threshold the saccade based on the depth of each sample (depthSaccade[]) using a fixed depth threshold: depthThreshold. 
	int maxAttemptsFollowing, attemptsFollowing_counter; // numbers of the spot will attempt to continue following contour even if moveAlongContour fails. 
	int maxAttemptsSearching, attemptsSearching_counter; // number of 
	boolean firstTimeStartRefraction;
        // Snell Law: 
        int countsign; //=1;
	float n1; // index
        // COMMON:
        boolean traceNormalVector;
        boolean displayDistance;
        boolean displayAngle;
        boolean displayMonocycle;

	float depth_Range_DEFAULT;
	//float Az_Degree_DEFAULT, El_Degree_DEFAULT;
	//float X_cm_DEFAULT,Y_cm_DEFAULT, Z_cm_DEFAULT;

	// Spheric coordinates (in degrees - and the radius is just depth_Range_Range in cm).
	float Az_Degree; // just equal to Az/oneDegree_DA 
	float El_Degree; // just equal to El/oneDegree_DA;
	// note: Az_Rad and El_Rad will be computed only as local variables in ComputeRangeUnconstrained()
	// Cartesian coordinates (in cm):
	//double X_cm, Y_cm, Z_cm;

	// SACCADE RADIUS (same comment on the separation of the dynamic parameters and the "geometrical" modelview parameters that should belong to a special "scanning/trajectory" class or structure. 
	float RadiusCorrectionDamping; // a small damping is simulated during the re-adjustement of the radius, to avoid abrupt oscillations (because of noise). 
	float Aper_Degree; // the current angular aperture of the saccade (in degrees) 
	//not used: float R_cm;// current radius of the saccade (in cm). REM: we need to compute the current distance to be able to calculate this. 	
	int RadiusCorrectionMode; // RADIUS_ADJUST or RADIUS_FIXED
	float delta_Aper_AD; // correction for the "aperture" radius in AD units (as a function of the distance). It is calculated in the function computeRadiusCorrection();

	// NETWON DYNAMICS (Verlet integration):
	boolean firstTime;// this is used to set the x(t-dt) for verdet integration the first time we update the position.
	float totalForceX, totalForceY; // acceleration is calculated form this, divided by the mass of the particle
	// bouncing force:
	float bouncingForceFactor; // this is for computing the force produced by bouncing on CONTOURS (at least when in newton mode), 	
	float centralForceFactor; // to compute the force given by a point of central attraction 
	float gx, gy; // this is the direction and norm of the "parallel" acceleration 
	float interForceX, interForceY; // the force given by interaction with other particles or objects (gravity+electrostatic), calculated in the main program
	float interForceFactor; // rem: the factors for this force are given by the mass and charge AND this additional factor
	float visForceFactor; // for calculating the viscosity force
	float rangeInteractionLong, rangeInteractionShort; //interaction length for attractive/repulsive force ( must be calibrated!! - potentiometer? based on spot diameter?)
	float mass;   // for calculating the gravitational force, as well as the acceleration
	float charge; // for calculating electrostatic force
	float dt; // this is the time interval in microseconds!
	float slowTime; //(the effective dt will be dt*slowTime)
	unsigned long oldTime; // this is to count dt (must be initialized). 
	// Interaction modes on/off, to avoid setting the constants to 0 if one does not want certain forces to work
	// (rem: all this only works in newtonian deltaElnamics mode!)
	boolean bouncingForceMode;// // this is bouncing by force generationtrue
	boolean bouncingSpecularMode;  // this is bouncing by inverting speed
	boolean centralForceMode;//=true; // default mode is CENTRAL force active
	boolean parallelForceMode;//=false; // default mode is parallel force inactive
	boolean particleInteractionMode; //= false;// default mode false
	boolean visForceMode;// =false; // default mode false

	// Saccade pre-processing, thresholding: 
	int thresholding_Mode; //define the use of a fixed or an automatic threshold
	float FIXED_THRESHOLD_FACTOR; // (???) this quantity (between 0 and 1) is used to set the "fixed" threshold (used when threshold mode is not automatic)
	float AUTO_THRESHOLD_FACTOR; // (0 to 1), controls the setting of the automatic threshold: auto_Threshold_Volts=(maxI_Volts-minI_Volts)*AUTO_THRESHOLD_FACTOR+minI_Volts (1/2 or 2/3 works fine...)
	float auto_Threshold_Volts; //threshold computed automaticaly to define dark and light area (voltage values)
	float fixed_Threshold_Volts; //current threshod used to define dark and light area (voltage values);// NOTE: it can also be copied/saved!!
	float depth_Threshold_Volts; // this is the value of voltage threshold computed when we set a given distance
      // then, the minimum required contrast, computed in RangeCalibration (because there I_mW_REF is computed).
	float MIN_CONTRAST_FACTOR; // a good value should be something like .1 (a tenth of the maximum contrast available...)
	//*** rem: MIN_CONTRAST is equal to this factor  MIN_CONTRAST_FACTOR x best attainable contrast (which is  just equal to I_Volts_REF - I_Volts_background; if I_Volts_REF is close to the 
	// saturation voltage, the value is something like 5 or 4 volts)
	int filtering_Mode; // can be LOW_PASS_FILTER or NO_FILTER, and is used to smooth data BEFORE binarization.
	boolean binary_smoothing_Mode; //NO_MORPHO_SMOOTH and MORPHO_SMOOTH (last means that rought data is smoothed using morphology operators before "zones" are calculated).

	// Recentering vector, angular calculation: 
	float error_angle; //correction angle due to the delay on the lockIn amplifier
	float deltaAz, deltaEl;   //this is the "recentering vector", normal to the interface, and pointing towards the dark part, computed after performing a saccade (this is done in the computeNormal function). 
	float NormVector; //Norm of (deltaAz, deltaEl)
	float nx, ny;     //normalized (deltaAz, deltaEl) vector
	float target_angle;// the direction of the recentering vector (in 

	// APD detection and saccade preprocessing:
	float I_Volts; // Current measure of the backscattered intensity in Volts (this is set to Imax_Volts when we finish a saccade)
	//float I_AD; 
	//float I_mW; // Approximate measured intensity in mW 
	float maxI_Volts, minI_Volts; // note: maxI_Volts and minI_Volts are defined as public variables now (easy access as relevant spot information). 
	float averageI_Volts, sigmI_Volts;
	//float averageI_mW, sigmI_mW;; // Approximate value in mW (just for presentation purposes)
	float average_maxI_Volts,  average_minI_Volts; // this is the average intensity of the "white zone" and the "dark zone" of the current saccade
	float whiteProportion;// this is the proportion of samples in the white zone in the saccade


	// Distance/Intensity model, geometrical parameters of the setup, etc: 
	float PD_X, PD_Y; // position of the avalanche photodetector window in the X/Y plane (origin is the galvano-mirrors) 
	int Distance_Intensity_Model; // the model for the distance/intensity 
	int exp_Cos; // the exponent to the cosinus in the case of "corrected lens" model, order 0
	float Attenuation_Factor_DEFAULT, I_Volts_background_DEFAULT; // Pre-calculated parameters (DistanceCm_To_Volts.m):
	float range_Adjust; // DEPRECATED!!

	// Parameters to record in EEPROM after calibration:
	//I_Volts_background_EEPROM 

	// Calibration (reference power, and distance): 
	// CalibrationParameters calibratedParameter;
        float I_Volts_OPTIMAL_REF_FACTOR; // sets the default I_Volts_OPTIMAL= I_Volts_OPTIMAL_REF_FACTOR*I_Volts_SAT (will be recalculated during calibration. 
        float depth_Range_REF; // in cm, is the REFERENCE CALIBRATION distance
         float R_cm_REF; // reference radius of the saccade (in cm), for a saccade depth_Range_Range_REF centimeters away from the system (for example, 600). 	
        float I_Volts_LowerMarge_REF_FACTOR, I_Volts_UpperMarge_REF_FACTOR;	// REM: I_Volts_UpperMarge=I_Volts_UpperMarge_REF_FACTOR*I_Volts_SAT, used to determine the quality of the calibration. 
        float CALIBRATED_MAX_DEPTH_RANGE; // this is the computed maximum depth_Range range in cm, such that if a point is computed to be farest than that, then it is considered lost because it is impossible to 
	// increase the laser power so as to get enough contrast. This depends on the choice of the lower limit (PD_mW_LowerMarge) for the acceptable intensity (see RangeCalibration).
	float CALIBRATED_MIN_DEPTH_RANGE; // this is the most proximal distance that does gives an acceptable level (still lower than the saturation
	// of PD), it depends on PD_mW_UpperMarge (see RangeCalibration). 
        // DEPTH RANGES: 
	float MANUAL_MAX_DEPTH_RANGE; // this is a MANUAL limit for the maximum depth_Range range in cm; if a point is computed to be fartest than that, it then we consider that the
	// trackspot is NOT hitting the target, and it does NOT have to move.
	float MANUAL_MIN_DEPTH_RANGE; // this is a MANUAL limit for the minimum depth_Range range in cm. Oclusion and other problems are taken into account here.
	// If a point is computed to be fartest than that, it then we consider that the trackspot is NOT hitting the target, and it does NOT have to move.
        // -------- PARAMETERS actually changed by the RangeCalibration routine (that should be saved or copied to other spots ----------------------------------------------------- 
	float RedLaserPower_REF; //reference laser power in mW, calculated in RangeCalibration. 
	//Rem: conversion bewtween volts and watts is very simple for the 1mW premium laser diode: 1V=1mW. 
	float I_Volts_OPTIMAL; // optimal intensity, that may or may not be attainable for the system (in BAD or FAIR calibration).
        float MIN_CONTRAST_Volts; // minimal variation of the intensity required to properly distinguish a light and a dark zone in the saccade, and then compute the recentering vector (delta_Az, delta_El)
	float Attenuation_Factor;//, sigmAtt; // = surface reflectivity x absorption mirrors x gain photodetector (area x sensitivity) x etc.
	float I_Volts_background, sigm_I_Volts_background;
	float I_Volts_maxLaser, I_Volts_minLaser;// maximum and minimum redout from the photodetector with maximum and minimum laser power.
	float I_Volts_REF; // this is the REFERENCE redout (in Volts) that MUST BE MANTAINED (LOCKED) by adjusting the laser power in order to have optimal contrast
        float I_Volts_UpperMarge, I_Volts_LowerMarge; // safe margins for the detected intensity (see RangeCalibration)
        float MAX_DEPTH_RANGE, MIN_DEPTH_RANGE; // the absolute max and min depth_Range range, calculated from the calibrated and manual ranges.
	// rem: at init, MAX_DEPTH_RANGE=MANUAL_MAX_DEPTH_RANGE, but after calibration the value can change to CALIBRATED_MAX_DEPTH_RANGE if this value was SMALLER.
        // ************ ADD TO THIS PARAMETERS to save this ones: 
        //float fixed_Threshold_Volts; 
        //float error_angle; 
        //--------------------------------------------------------- -----------------------------------------------------  -----------------------------------------------------  

	// MIRRORS (zone delimiters, etc): 
	float xMax, yMax, xMin, yMin; //border of the mirrors (normaly, min=0, max=4000) in AD units
	// REM: this could be used to set values in the hardwareIO object

	// BORDER RANGE MODES:
	boolean borderDamping;
	int borderMode; // can be: NORMAL_REFLEXION;//RESET_POSITION;// RETRO_REFLEXION;
	boolean showLimits;
	boolean showGreenFlash;
	
	// RED LASER: 
	int PowerCorrectionMode; // either POWER_FIXED or POWER_ADJUST 
	float delta_RedLaserPower; // correction of the laser power. 
	float min_delta_RedLaserPower; // minimum correction for the laser power (used in the gradient descend stop test).
	float LaserCorrectionDamping; // this is to simulate a small damping when re-adjusting the laser power, so as to avoid abrupt oscillations (because of noise).
	float RedLaserPower; // current laser power (in mW)
	//float MAX_RedLaserPower, MIN_RedLaserPower; // max and min admissible powers for the red laser when tracking (now made #define in hardwareIO.h)
	// (these values are used during calibration, but also as a way to limit the value of the variable RedLaserPower, so calculatio of the real depth_Range is not affected - from the readout of photodetector) 
	//float interscanTrajectory_RedLaserPower; // `the interscanTrajectory_RedLaserPower may not be equal to 0, to avoid having the lock-in loosing the signal phase. 

	// GREEN LASER: 
        boolean GreenLaserPower; // HIGH or LOW (true or false)

	// New (17.7.2010): flag to compute range per-sample basis (using laser power updated per-saccade basis: this is a preliminary solution that may work if the saccade does not have wild variations
	// on its intensity):
	boolean computeDepthPerSample; // when true, the depthSaccade[] array will be filled in the analyzeSaccade function. 

	// Position buffers (to calculate FFT, detect circles, etc). Rem: the buffer can be "smoothed".
	// NOTE: on ATmega1280, we only have 8K Bytes Internal SRAM !!!
	// float Position_Buffer_Cart[POSITION_MAX_BUFFER_SIZE][3]; 
	// etc.
	// ***** TO DO ******

	// Variables for computing the curvature radius and related stuff:
	//float Speed_cm[3]; // the vector instant speed
	//float Accel_cm[3]; // the vector instant acceleration
	// etc.
	// ***** TO DO ******

	// Results of position buffer analysis: 
	// float radiusGyration; // not used for the time being: this can be time consuming in Arduino
	// int detectedCorner; // perhaps time consuming (uses radiusGyration)
	// ***** TO DO ******

	// Information to be sent by OSC:
	//CSpotInformation *spotInfo;
	// *** TO DO ***


	//public Methods

	// Constructors, initializers ------------------------------------------------------  
	//LivingSpot(int posX = 2000.0, int posY = 2000.0, int saccadeRadius = 100, int numSample = 18, int id=0); //full parameters constructor
	void Init(float posX, float posY, float saccadeRadius, int numSample, int id);

	// GLOBAL UPDATE function  ------------------------------------------------------ 
	// that will call one of the spot behaviours above as a function of the current behaviour mode:
	void update(LivingSpot* spotArray, int numSpots);

	// Living Spot BEHAVIOURS and related functions ------------------------------------------------------ 
	int lightRefraction();
	float refractionIndex(float intensity); // simulated refraction index from measured intensity
	int newtonDynamics(int numSpots); 
	int computeInteraction(LivingSpot* spotArray, int numSpots); // used for particle interaction when in newtonian dynamics mode
	int fastBouncing(); //go straight until a dark edge, perform a saccade and bounce according to the reflection direction
	int scanBouncing(); //scan and bounce on dark edge
	int simpleBouncing(int numSpots); // simple bouncing mode, doing a saccade all the time
        int simpleBouncingCentralForce(int numSpots); // a substitute to newtonian interaction that does not work yet
	boolean moveAlongContour(int numSpots); // this function moves along the direction perpendicular to the contour (at the instant it is called) by a factor of the diameter
	boolean searchContour(int numSpots);
        void standBy(void);
        void tracking(void);// returns value?

	// CALIBRATION and check routines ------------------------------------------------------ 
	int RangeCalibration(void);  
	void checkRecenteringVector(float azp=CENTER_AD_MIRROR_X, float elp=CENTER_AD_MIRROR_Y, float aperp=40);
        void drawMaxArea(int nloops = 2); //draw a squarre corresponding to the maximum area
	// ** TO DO: set frequency, etc. 


        // SCANNING/DISPLAY  ------------------------------------------------------ 
	// scanning/drawing saccade and computation of the binary data table:
	int scanTrajectory(byte *saccadePosCenteredAndNormalized, int numsamples, float centAz, float centEl, float radius, float rotation); 
        int scanTrajectory_Letters(byte *saccadePosCenteredAndNormalized, int numsamples, float centAz, float centEl, float radius, float rotation);

	// Process saccade data ------------------------------------------------------ 
	int analyzeScanData(int numsamples);
	float computeAutoThreshold(float maxVoltsMeasured, float minVoltsMeasured);
        float computeAutoThreshold(); // overloaded function using the current statistic of the saccade (in particular constrast flags) 
	//Related to the circular saccade:
	boolean computeNormalVector(float az, float el, float radius, float rotation=0); //compute the normal vector (particularly meaninful when the saccade is circular). 
	void showLetters(void); // similar to computeNormalVector, but without the additional saccade points as in the circular saccade
        boolean computeAngularCorrection(void); // called by computeNormalVector
        boolean computeAngularCorrection_NEW(void);

	// Distance-to-intensity and intensity-to-distance calculation ------------------------------------------------------  
	float ComputeRangeUnconstrained(double paramI_Volts, double paramRedLaserPower);
	float ComputeRange(float paramI_Volts, float paramRedLaserPower); //computes depth_Range, taking as input parameter the maximum acquired intensity during the saccade in volts (maxI_Volts or average_maxI_Volts), 
	// as well as the current laser power. Output is corresponding distance (in cm).
	float computeIntensityVolts(float range, float paramRedLaserPower);
	void computeDepthSamples(float saccade_redlaserpower, int numsamples);
	void updateDepthThreshold(float range);

	// Laser Power related ------------------------------------------------------ 
	// Laser Power correction (proportional control), to avoid lockin saturation: 
	float LaserPowerCorrection(float depth, float redLaserPower); // this computes the value for the delta_Laser, so as to mantain a constant readout for the photodetector 
        // intensity (we do that by using a damping factor and converging to the proper value).  
        int correctLaserPower(void); // actually correct the laser power (do clipping). 
        // void setRedPower() ; a function that changes the current red laser power (saccade)

	// Radius setup and automatic correction ------------------------------------------------------  
	float computeRadiusCorrection(float depth); // this computes the values of the delta_Aper_AD as a function of the computed depth
	void resetRadius(void);// re-initialization of the  angular aperture of the saccade from reference distance and reference saccade size
	void updateRadius(void);// update the saccade radius as a function of the current computed distance (a call to computeRadiusCorrection) will give the additive correction delta_Aper_AD 

	// Motion ------------------------------------------------------ 
	void moveUncheckedSpot(float posX, float posY); // this just move the mirrors (it's an alias for IO.setMirrors_XY(int(), int())
	int moveSpot(float posX, float posY); //update the position of the spot (check limits by calling updatePosition) and actually move it
	int updatePosition(float posX, float posY); //update the current position of the spot (Az, El), by doing border checks.
	void updateSpeed(float speedX, float speedY); // this is necessary in the case of Newtonian dynamics. 

	// Generation of the position array for the scanning trajectory ------------------------------------------------------  
	// Circular saccade: 
	int createCircularSaccade(int numSamples);//sets the number of samples, and update the saccade table accordingly
	// Logos: 
	//void drawSLPLogo(int centerLogo_X = 2000, int centerLogo_Y = 2000, int sizeLogo_X = 80, int sizeLogo_Y = 80, int nloops = 1); //draw logo
	//void drawSLPLogoAngle(float angleLogo, int centerLogo_X = 2000, int centerLogo_Y = 2000, int sizeLogo_X = 80, int sizeLogo_Y = 80, int nloops = 1); //draw the logo oriented with a custom angle
	//void drawSLPLogoRotating(int speedRotation = 2, int centerLogo_X = 2000, int centerLogo_Y = 2000, int sizeLogo_X = 80, int sizeLogo_Y = 80); //draw a rotation SLP Logo on a complete 360 deg loop

	//Transformation on the saccade ------------------------------------------------------ 
	// (1) translation: this is Az, El
	float getAz(); //return the value of the protected variable Az
	float getEl(); //return the value of the protected variable El
	// (2) Size: 
	int setAperSaccade(float pAperDA);  // will change Aper_DA, and take care that the new radius does not exceed a certain ad-hoc value, nor is negative (in such case, the function return -1)
	int setRadiusSaccade(float Rcm);    // change the reference radius (i.e., makes the saccade with constant radius Rcm)
      // NOTE: in fact, the saccade radius can be considered as the "size" of ANY drawing!
	// (3) rotation, modelview, warping, etc...
	// ... to do

	// Process position buffer ------------------------------------------------------ 
	//void ComputeRadiusCurvature2D();
	//void ComputeRadiusCurvature3D();
	// *** TO DO: FFT, spikes, etc

	// SEND RELEVANT DATA to other computers ------------------------------------------------------ 
	// (for sound generation, etc). This may be better called in the "behaviour" code
	//void UpdateSpotInfo();
	//CSpotInformation *GetSpotInfo();
	void sendSoundData(void); 
	// *** TO DO: send OSC data from spot info structure

	// Miscelanea methods  ------------------------------------------------------ 
	int set_circularSaccadeFromChar(char charTrajectory);

        // TEXT DISPLAY:
   //void drawTrajectoryFromString(char *trajectory, int nbChar, boolean centered, int RedLaserPower, boolean GreenLaserPower, float offsetAz, float offsetEl, float radius, float rotation);
    void drawTrajectoryFromString(char *trajectory, boolean centered, float pRedLaserPower, boolean pGreenLaserPower, float offsetAz, float offsetEl, float radius, float rotation);
  void drawTrajectoryFromChar(char trajectory, float pRedLaserPower, boolean pGreenLaserPower, float centAz, float centEl, float radius);
  void drawTrajectoryFromChar(char trajectory, float pRedLaserPower, boolean pGreenLaserPower, float centAz, float centEl, float radius, float rotation);

private:

	//NOTE: for some strange reason, the STATIC keyword does not work in a class compiled for Arduino??? This means that the number of spots should be inferred from the length of the spotArray, 
	// probably using a global variable in the main program... ugly.
	// static int numberOfSpots;//=-1; // this variable will be shared by ALL the instances of the class, and its initialization is done ONLY ONCE. 
	// its value will be increased each time we instantiate a new object, and decreased each time we delete one.

	float Az, El; //current position
	///float vx, vy; //current speed 


	/*
	int *_circularSaccade; //integer table containing the position of each sampling spots (created by 'void updateSaccadeTab()') (double size one dimension table for X and Y)
	int *_rawScanData; //integer table containing the value of the lockIn Amplifier for each sampling spots (created by 'void updateSaccadeTab()')
	byte *_binarizedScanData; //binary table of the saccade (0 = dark area; 1 = light area)
	byte *_auxBinarizedScanData; //binary table used by erode() and dilate(), binary transition vector
	*/
	// Saccade array positions: 
	byte _circularSaccade[2*MAX_SACCADE_SAMPLES]; // array containing the position of each sampling spot in the circular saccade (created by 'void updateSaccadeTab()') 
	int _numSamplesCircularSaccade; //number of sampling spots for the CIRCULAR saccade (distributed uniformly around the saccade circle), should be <=MAX_SACCADE_SAMPLES
        
       // float _arbitrarySaccade[2*MAX_SACCADE_SAMPLES]; // for arbitrary saccades
       // int _numSamplesArbitrarySaccade; //should be <=MAX_SACCADE_SAMPLES
	
	// Saccade array data (acquired lockin voltage):
	// Note: these variables will be used in the analyzeScanData and computeNormVector functions; in any case, we won't have a separate storage space for 
	// arbitrary saccades. 
        // ATTN: since 20.7.2010, this is a GLOBAL variable. This is just because we don't have ENOUGH SRAM on the arduino...
	//float _rawScanData[MAX_SACCADE_SAMPLES]; //float array containing the value of the lockIn Amplifier (in Volts 0-5) for each sampling spots (created by 'void updateSaccadeTab()')

	// Auxiliary array for processing (low pass filter, etc):
        // ATTENTION: to gain RAM SPACE, I use depthSaccade[] as the auxiliary array. This is possible because we always filter FIRST (if required), and then (if required) compute per-sample depth. 
	// float auxDataProcessing[MAX_SACCADE_SAMPLES];

	boolean _binarizedScanData[MAX_SACCADE_SAMPLES]; //thresholded saccade array (false = dark area; true = light area for each saccade point)
	byte _auxBinarizedScanData[MAX_SACCADE_SAMPLES];//binary table used by erode() and dilate(), binary transition vector

	float depthSaccade[MAX_SACCADE_SAMPLES];// array to store the computed depth for each point of the saccade.
	// Read note on my notebook 17.7.2010: since appropiate laser power is needed for measuring a proper intensity (we are not using phase-rangefinding), a preliminary solution 
	// not involving the per-sample adjustement of the power is to use the saccade-global laser power RedLaserPower (or RedLaserPower_Saccade in the future) which is calculated taking the
	// measured I_Volts intensity of the sample that gives larger intensity, or the average on the whole saccade, or average on the "white" zone. Then, we can call computeRange for each point in 
	// the saccade assuming that we will have a good intensity readout for all the points (_rawScanData values) if the saccade is not very large.  

	//variable for fastBouncing
	const static int _nbAvr = 4;
	float _averageLockIn;
	float _avLockInTab[_nbAvr];
	int _idAv;	
	int _idSaccade;

	//private Methods:
        // ...
	
	// Miscelanea functions   ------------------------------------------------------ 
	int LowPassFilter(int numsamples);
	void _Dilate(int);
	void _Erode(int);

};

#endif

